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Abstract: This paper describes the Spatial Operator Algebra framework for the dynamics of general 
multibody systems. The use of a spatial operator-based methodology permits the formulation of the dy- 
namical equations of motion of multibody systems in a concise and systematic way. The dynamical equations 
of progressively more complex rigid multibody systems are developed in an evolutionary manner beginning 
with a serial chain system, followed by a tree topology system and finally, systems with arbitrary closed 
loops. Operator factorizations and identities are used to develop novel recursive algorithms for the forward 
dynamics of systems with closed loops. Extensions required to deal with flexible elements are also discussed. 


1 Introduction 


The field of multibody dynamics is currently being challenged in two major ways. The increase in the size and 
complexity of spacecraft systems requires the development of tools that not only help manage the complexity 
of such systems, but also facilitate the development of novel dynamics formulation techniques and solution 
algorithms. Areas such as robotics involve multibody systems consisting of multiple robot manipulators 
interacting with each other and with complex environments. These are multibody systems with not only 
constantly time-varying topological structure, but also ones in which the constituent bodies change with 
time. Coping with this aspect requires versatile and flexible dynamics simulation tools. 

In this paper, the Spatial Operator Algebra Framework [1] is used to develop a systematic procedure 
for concisely formulating the equations of motion and derive spatially recursive forward dynamics algorithms 
for multibody systems. The equations of motion of progressively more complex rigid multibody systems such 
as serial chains, tree topology systems and finally closed chain systems are developed. Operator factorizations 
and identities are then used to obtain efficient spatially recursive algorithms for the forward dynamics of 
such systems. Extensions to handle flexible link elements are also discussed. 


2 Equations of Motion 


We begin by briefly describing the coordinate-free spatial notation used throughout this paper. Given the 
linear and angular velocities t; and u, the linear force F, and moment N at a point on a body, the spatial 
velocity V, spatial acceleration a and the spatial force f in 'll 6 are defined as follows: 


vk 




The rigid body transformation operator £ 72 6x6 is defined as: 
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where / is a vector joining two points, and / is the cross-product matrix associated with / which acts on a 
vector to produce the cross-product of / with the vector. and <£*(/) transform spatial forces and spatial 
velocities respectively between two points on a rigid body seperated by the vector /. For a rigid body, its 
spatial inertias Me and Mo at its center of mass C and at another point O respectively, are defined as 

"< c > g ( T * mi ) • "<°> g tirMWtr) = ( J }°1 Z ) 

where p is the vector from O to C, m is the mass of the body, and J(C) and J(0) are the inertia tensors 
for the body about C and O respectively. The reader is referred to [2] for additional discussion on the use 
of spatial notation. 


2.1 Dynamics of a Serial Rigid Multibody System 


Serial rigid multibody systems form basic subsystems from which the dynamics of more general rigid multi- 
body systems can be generated. In this section we derive the equations of motion of a serial multibody system 
consisting of n rigid links connected together by multiple dof joints. The links are numbered 1 through n 
from tip to base. We use the terms outboard ( inboard) link to refer to a link on the path towards the tip 
(base). 


The set of configuration variables for the serial chain are the collection of the joint configuration 
parameters. It is assumed that the k ih joint possesses r p (k ) positional dofs parameterized by the vector of 
configuration variables 0(k) (of dimension at least r p (ib)), and that its r v (k) motion dofs are parameterized 
by the r v (k) dimensional joint velocity vector /?(£). The kinematical equations which relate 9{k) to /3(k) 
depend on the specific nature of the k th joint. It is assumed for notational convenience that all the joint 
constraints are homogeneous (i.e., catastatic). H(k) is defined such that H*(k) is the 6 x r v (k) joint map 
matrix for the k th joint whose columns span the space of permissible relative spatial velocities Ay(k) across 
the joint. The complexity of the dynamics algorithms for the serial chain is determined by the number of 

overall motion dofs M = J2k = l r v(k) for the chain. The state of the multibody system is defined by the 
collection of [#(.),/?(.)] for all the joints, and is assumed known. 


Since each link is rigid, it suffices to develop the equations of motion at a single reference point 
on each link, which is taken to be the inboard joint location Ok for the k th link. With V(k ) denoting the 
spatial velocity, a(Ar) the spatial acceleration, f(k) the spatial force and T(k) the joint force at Ok for the 
k ih link, the following Newton-Euler recursive equations describe the equations of motion for the serial rigid 
multibody chain: 


V(n+ 1) = 0, a(n+l) = 0 
for k = n • • ■ 1 

V(k) = + l f k)V(k + 1) + H*(k)/3(k) 

a(k) = + l,k)a(k + 1) + H*(k)0(k) + a(k) 

end loop 


[ m = o 

for k = 1 • • • n 

< /(*) = + 1 , k)f(k — 1 ) + M(k)a(k) + b(k) 

T(k) = H(k)f(k) 

end loop 


( 2 . 1 ) 


a(k) and b(k) are the velocity dependent Coriolis acceleration and gyroscopic forces repectively for the 
k th link at Ok . <f>{k,k - 1) denotes the transformation operator from Ok-i to Ok . For additional details 
regarding the derivation of these equations of motion, see [1]. We have made the simplifying assumption 
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that the tip force /( 0) is zero. Attaching a full 6 motion dof joint between the physical base and the inertial 
frame allows us to easily deal with the mobile base situation. For the inverse dynamics problem, the joint 
accelerations 0 are known, and Eq. (2.1) represents an 0(/f) computational process involving a base-to-tip 
recursion to compute the velocities and accelerations, followed by a tip-to-base recursion to compute the 
joint forces. 

In order to express the equations of motion given by Eq. (2.1) in a more compact form, we define 
the stacked notation. In this notation, the V(k)’ s, a(k)’s etc are viewed as components of vectors V, a etc. 
Then Eq. (2.1) can be written in the following compact form: 
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0 ... H{n) ) 

However, since £ 4 , is nilpotent ( = 0), 

<j> — (I — £<f,)~ l = / 4 - £<t> + £% + 




/ 1 

*( 2 , 1 ) 
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where, 

*(»',;) = *(*,»- 1) 

Thus Eq. (2.2) can be reexpressed in the form, 

V = **tf*;3 

= f(H'0 + a) 

= <f>(Ma + b) = <f>M<f)*H*0 + + b) 

= Hf = H<j>M<t>*H*(3+ H<}>{M<pa + b) 


\ *(n,l) *(n,2) 

*(i + i,i) 


0 \ 

0 

1 ) 


(2.4) 


a 

f 

T 


(2.5) 


= M0 + C, where M = H*M** H* , and C = H </>(M ** a + b) 


M G 1 is the mass matrix for the serial chain and C € consists of the velocity dependent Coriolis, 
centrifugal and gyroscopic joint forces. In the terminology of Kane’s method [3], (3 are the generalized speeds 
and the elements of <f>* H* are the partial (spatial) velocities. 

£$, 0, H, and M are the first of the spatial operators that will be encountered. Recursive dynamical 
algorithms can be derived naturally by exploiting the special state transition properties [1] of the elements of 
spatial operators such as £^ } <j> etc.. For instance, given a vector y, the evaluation of the matrix-vector product 
4 >y does not require an 0(n 2 ) matrix-vector product computation, and not even the explicit computation 
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of the elements of <£, but rather, it can be evaluated using an O(n) recursive algorithm involving only the 
elements of £$ and y. This is precisely the correspondence between the concise operator based high-level 
description of the equations of motion in Eq. (2.5) and the recursive algorithmic description in Eq. (2.1). 

Spatially recursive 0(N) forward dynamics algorithms for serial chains have been developed in 
[4] based on the recognition of the isomorphism between the structure of the dynamics equations and the 
equations encountered in Kalman Filtering theory. These insights have formed the basis for the development 
of the Spatial Operator Algebra Framework for multibody dynamics. 


2.2 Tree Topology Systems 


In this section, the dynamics of rigid multibody systems with tree topological structure are discussed. A 
tree topology system may be viewed as a set of component serial chains (referred to as branches) coupled 
together via joints at their terminal links. The total number of branches is denoted i. The index for the 
branches thus ranges from 1 • • • l, and consistent with the link numbering scheme in the previous section, 
the inboard branches are assigned indices larger than those for the outboard ones. The connectivity function 
i(k) is defined as the index of the direct predecessor branch, i.e., the inboard branch to which the k ih branch 
is connected. The j th branch is simply denoted a predecessor branch for the k th branch if it belongs on the 
unique path from the k ih branch to the base, i.e., if i p (k) = j for some integer p > 0. The joint coupling two 
branches is assigned to the outboard branch. Figure 1 illustrates the link/branch numbering convention for 
tree topology systems. 


The notation for serial chains from Section 2.1 is carried over to describe the branches in the tree 
structure, and an additional subscript is used to identify the specific branch in the system. Thus nj and Mj 
denote the number of links and the number of motion dofs respectively, while V}, Mj, 8 < j >i , <f>j etc. denote 
the appropriate spatial velocity etc. quantities for the j th branch. A link/joint is identified by the index 
of the branch it is on, plus its location within the branch. For instance, V(kj) (or more accurately Vj(k)) 
denotes the spatial velocity of the the k th link of the j th branch at its inboard joint location O(kj). The 
overall stacked spatial velocity, acceleration etc. vectors for the tree are now denoted V, a, / etc. with 

V = [Vy • • • V£]* etc.. The total number of links n, and the total number of motion dofe M for the system 
are given by 

n= anc * ^ ^ (2.6) 

;=i i=i 

Note that when the j th branch is the direct predecessor of the k th branch, i.e., j = i(k) J the joint connecting 
them is the nJ. A joint on the k th branch and is located on link lj on the j th branch. The transformation 
operator from the joint to the lj* joint is denoted n*). The spatial operator £$ is now defined in 
terms of its block matrix elements below. For j,k G !•••£, 


£<t>j 


*♦(**)=< 


/ o • o 
0 0 

^ 0 ••• 0 


o 

o / 


for j = k 

for j = t(k), i.e. if j is the direct predecessor branch of k 


( 0 for j / *(fc), i.e. if j is not the direct predecessor of k 

(2.7) 

0 denotes a zero matrix of dimension appropriate for the context. As a consequence of the numbering scheme 
used here, for j < k y the j th branch cannot be a predecessor to the k th branch and thus the (j, k) th block 
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element, £<f>(j : &) = 0. Thus £$ is a strictly lower triangular matrix. The analogs of Eq. (2.2) are as follows: 



v = s;v + h*p 



a = £^Qt 4 H* ft 4 u 



/ = £^f 4 Mol 4 b 

(2.8) 

Once again (analogous to Eq. (2.4)), £$ 

is nilpotent (££ = 0), and so 


1 

<11 

— l + £<t> + £4>+ **• + £# 1 

(2.9) 


The block structure of <f> is described below: 


* <f>j for j = k 

{<t>(mj 1 lk)}m,l if 3 p > 0 : j = i p (k) y i.e., if j is a predecessor branch of k (2.10) 

0 if j ^ i p (k) V p > 0, i.e., if j is not a predecessor branch of k 

Here {<l>(mj ,lk)}m,l denotes a block matrix whose ( m,l) th entry is given by <}>{mjjk) with m G 1 • * rij and 
l £ 1 • nfc. <j>{mjjk) is the transformation operator from joint Ik (on the k ih branch) to joint mj (on the 
j th branch) and is a generalization of the transformation operator in Eq. (2.4) for serial chains. It 

is formed by sequentially composing all the individual transformation operators that lie on the unique path 
joining the two joints. The numbering scheme used here ensures that <j) will be a lower triangular matrix. 
The operator <f> has state transition properties analogous to the <f> for serial chains, and as a consequence, it 
can be used for high-level and concise description of the dynamics of tree topology systems (as in Eq. (2.11) 
below), but with the full understanding that from the computational perspective, these equations directly 
map into recursive implementation procedures. From Eq. (2.8) and Eq. (2.4) it follows that, 

V = 

a = **(#*/? + a) (2.11) 

/ = <j>(Ma + b) = H* ft + <f>(M<t>*a 4 b) 

T = Hf = H<f>M<t>*H*j3 + H<j>{M^a 4 b) 

— Mft + C, where M = H H* , and C = H <j)(M <j>* a + b) 

M G 'JZ J ^ xAr denotes the mass matrix for the tree system. T = T — C can be easily computed from the 
knowledge of the system state, and so the equations of motion for the system can be rewritten in the form 

Mfi = T ( 2 . 12 ) 

The forward dynamics problem requires then the solution of the joint accelerations ft for a given set of joint 
forces T. The mass matrix for the system is typically not available and potentially needs to be computed to 
solve the forward dynamics problem. However, in Section 3, a recursive 0( Af) forward dynamics algorithm 
for tree topology systems, which does not require the explicit computation of the mass matrix M, is derived. 

Before proceeding on to closed topology systems, we first derive the structure of the Jacobian 
operator. Given nc points, denoted C*’s, on the tree (see Figure 1), the Jacobian operator J G 'JZ 6ncxAf 
defines the mapping between /? and V , i.e., V = J/?, where V G ft 6nc denotes the vector of spatial velocities 
at these points. If C* is on link mj , then the spatial velocity at C k is given by 

V(k) = f(G(m j ) ) C k )V(rn j ) 

with <K0(rnj),Cjb) denoting the rigid body transformation operator from C k to the point 0(rrij). With the 
block elements of B G ft 6nx6nc defined as 

( <t>{0(rrij),Ck) if C k G rrij h link 

£(mj,Jk)=< for k = 1 *• n c (2.13) 

| 0 otherwise 
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it follows that 


V = B*V = B'fH'p, i.e., J = B*<j>* H* (2.14) 

This gives us an expression for the desired Jacobian operator which will be used below when dealing with 
loop closure constraints for closed topology systems. 


2.3 Closed Topology Systems 


This paper develops a systematic procedure for the formulation of the equations of motion and derivation of 
forward dynamics solution algorithms for general topology multibody systems with time-varying topologies 
as well as changing constituent bodies. Based on the specific application, such systems may be conceptually 
partitioned as follows: 

(a) The primary system consisting of the least time-variant part, i.e., the multibody subsystem with fixed 
topology and constituent bodies. 

(b) The secondary system consisting of the multibody subsystem which may change from time to time. 

(c) The set of closure constraints and/or boundary conditions between/within the primary and secondary 
systems which change with changes in the system topology. 


Note the the subsystems described above are in the order of increasing time-variation. As an example, let 
us examine the robotics scenario of multiple manipulators interacting with each other and the environment 
to perform complex tasks. In this context, the manipulators belong to the primary system since their innate 
structure varies very little with time. The task objects vary from task to task and form the secondary 
system. The constraints between these two subsystems change during the execution of a task, such as 
grasping, mating, tool operation etc., and belong to the last category. 

This partitioning allows us to derive a very general and yet systematic procedure for the development 
of dynamics algorithms which are responsive and adaptable to time-varying systems. The procedure involves 
a sequence of decoupled steps for each of the primary and secondary system dynamics, and one step in which 
they come together when the constraint forces are computed. Being structurally time-invariant, it is possible 
to put in place optimized algorithms for the dynamics of the primary system. The time-variant secondary 
system is typically of small complexity and thus the use of standard, though suboptimal, algorithms does 
not substantively degrade performance. 

This decomposition of the closed topology system is a departure from the more traditional approach 
(see [5, 6]) of forming a spanning tree for the full system and computing the constraint forces at the points 
of closure. In these latter approaches, even small changes in the original system typically lead to whole new 
spanning trees for the system. This disallows any algorithmic optimization, and the algorithms are also not 
very amenable to coping with time-varying systems. 

The primary and secondary systems in most applications have tree topological structure. However 
in general there may be internal closed loops within either system. In any case, by cutting an appropriate 
number of joints, each subsystem may be regarded as a tree topology system with additional kinematical 
constraints at the internal loop closure points. The equations of motion for tree topology systems derived in 
Eq. (2.12) will be used to describe the dynamics of the tree components of both the primary and secondary 
systems, with the subscripts “P” and “S” differentiating the two subsystems. Thus the dynamics of the tree 
part of the two systems are described by 

Tp = Hp<f>pMp<f>* P Hpj3p = Aipfipj and T$ — H $<i>sMs<t>*sHsfis ~ MsPs (2.15) 

Mp and Ms denote the mass matrices, /3p and (3s the motion dof parameter vectors, Tp and Ts the 
bias-free internal joint forces for the primary and secondary subsystems respectively. 
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Combining the internal loop points of closure with the points of closure coupling the two systems, we 
obtain the overall points of closure for each of the subsystems. Let Vp and Vs denote the spatial velocities 
at these overall points of closure for the two systems, and following the discussion leading to Eq. (2.14), let 
J P - Bp<f>* P Hp and J s = B* s <f>* s H* s denote the Jacobian operators for the two systems corresponding to 
these points. Thus V P = JpPp and V s = J s fis- The kinematical constraints due to the existence of internal 
closed loops within the primary and secondary systems leads to constraint equations of the form: 

QpVp = Up and Q S V S = U S 

The coupling together of the primary and secondary systems via joints leeds to constraint equations of the 

form: _ _ 

QpVp + QsVs — Uc 


Defining 



As 


A / Qs \ 

\ Qs )' 


and A = [Ap As] 


the closure constraints can be collectively expressed in the form: 


A {% ) -H, *]( J ; £ )(£ ) )- ( | ) =0 


It is assumed that [A P J P A 5 J.s] is of full row rank Mb- The overall number of motion dofs of the closed 

chain system is given by Me = M + Ms — Me, a nd if necessary, Eq. (2.16) can be used to find the Me 
dimensional minimal set of motion generalized coordinates for system. Based on the principle of virtual 
work, Eq. (2.16) implies that the closure constraint joint forces are of the form 



/ 


for some / € RM e . This leads to the following overall equations of motion: 


( M v 0 J P A P \ / fa \ / T P \ 

0 Ms J's^s /?5 = Ts , 

y A P J P AsJs 0 / \ f J \ U / 


=> 


( Mv 0 
0 Ms 
0 0 


r P A* P 


J' S A' S 

— [j4pApj4J> + AsAsAJ] 


where U = U — \{A P J P ) (As Js)] ( ) 

fa \ ( T P 

fa = _ Ts 

/ / \ U — [A p J p M P l T p + AsJsMg Ts] 

(2.17) 


where 

Ap = J p-Mp 1 J P , and As = JsM$ 1 Js 

Note that Ap and As are the effective “admittances” of the primary and secondary systems reflected to the 
points of closure. We now describe some special cases of the above setup: 


• The joint constraints coupling the primary and secondary systems are typically on the relative spatial 
velocity across the joints at the points of closure. When this is true for all joints, an appropriate 
reordering of the elements of V will result in Q P = -Qs- Furthermore, if no relative motion is 
permitted across the joint, i.e., there is rigid rather than loose coupling, then in fact Q P = / and 
Q s = I. When this is the case for only some of the joints, only the corresponding rows have these 
special features. 

• If the secondary system has no internal actuators or source of generalized forces, then T s = 0. 

• If the secondary system is a free rigid body with no internal degrees of freedom, then the motion 
generalized coordinates vector /3s is of dimension 6 and consists of the 3 translational and 3 rotational 
dof parameters. 
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3 Forward Dynamics of Closed Chain Systems 


In this section we discuss a recursive method for solving the forward dynamics of closed chain rigid multibody 
systems. This method does not require the explicit computation of the primary and secondary tree system 
mass matrices Mp or Ms, but does require the computation of the constraint force parameters. 

From the equations of motion of the closed chain system with dynamical closure constraints given 
by Eq. (2.17), the solution of the forward dynamics problem can be solved by the following sequence of steps: 


(A) 

Solve Mpfif =Tp for /?£ 

Solve MsPs = Ts for /?£ 

(B) 

Compute a* p = Jpfip 

Compute a* s = JsPs 

(C) 

Compute Ap = JpMp X Jp 

Compute As = JsMs l Js 

(D) 

Solve [ApApAp + A 5 A 5 AJ]/ = 

(A P a f p + A 5 a£) - U for / 

(E) 

Solve M P p b p = -r p A*pf for /?£ 

Solve MsPs — -Js^'sf f° r 

(F) 


Ps=% + 41 


As a result of the partitioning, a changes in the closure constraints only effect A and thus only STEP 
D, while changes in the secondary system effect only the steps in the right half column. Recursive algorithms 
for carrying out each of these steps are derived below. The proofs of the various lemmas are omitted due 
to space limitations. However they follow precisely along the lines of the proofs for serial chains discussed 
in [7]. The explicit use of the subscripts indentifying the primary /secondary system is dropped (except for 
STEP (D)) since the discussion is equally applicable to either subsystem. 


STEP (A) Solve MP* = T . (Forward Dynamics of a Tree Topology System ) 

Note that Step (A) is equivalent to solving the forward dynamics of a tree topology system, and we 
develop an 0(N) recursive algorithm for this solution. This algorithm is based on a new factorization 
of the mass matrix M in terms of square factors, which may be contrasted with the earlier non-square 
factorization in Eq. (2.11). This square factorization is then used to obtain an explicit expression for 
Ad" 1 . 

The articulated body inertia matrix P is defined as the solution to the following equation: 

M = P- £+[P - PH*{HPH*)- l HP]£l (3.1) 

P is block diagonal and the elements on the diagonal (denoted P(kj)) can be obtained using a recursive 
algorithm described in Eq. (A.l) in Appendix A. Physically, P(kj) is the articulated body inertia as 
seen at the kj h joint, i.e., it is the effective inertia of all the links outboard from the kj h joint assuming 
that the joint forces at all the outboard joints are zero. 

For the subsequent development, it is convenient to define 

D = HPH\ G = PH*D~\ K = S^G 

t = GH, t = I - t, £^ = £$t (3-2) 

Note that D , G, r and 7 are all block diagonal. The structure of £g, is identical to that of £$ with its 
elements being given by 

tfiikj'kj - 1) i *(*,.*, - l)r(kj - 1) 
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(3.3) 


e+ is also nilpotent (££ = 0), and analogous to <p, ip is defined as 

rj> = (i- e*)- 1 = /+ £* +£* + ••• + *r‘ 

The structure of ip is very similar to that of <p and it also possesses the state transition properties which 
are used to develop recursive algorithms. <j> may be viewed as the transformation operator for composite 
bodies (i.e., as if all the joints are locked), while xp is the transformation operator for articulated bodies 
(i.e., as if all the joint forces were zero). The following lemma yields a square factorization of M. 

Lemma 1: The mass matrix M has the following factorization: 

M = [I+H<f>K]D[I + H<j>K ]*, (3.4)1 


The following lemma gives the explicit form for the inverse of [7 + H<pK]. 


Lemma 2: 

[7 + H4>K]~ X = [I- HipK] (3.5) I 

Combining Lemma 1 and Lemma 2 leads to the following form for the inverse of the mass matrix. 


Lemma 3: 


AT 1 = [/ - H*K)'D- l [I - HrpK] 


(3.6)1 


Thus, 

ft = M-'T = [7 - HipK]* D _1 [I - Hj>K]T (3.7) 

The 0(Af) recursive computation of the expression on the right is given in Eq. (A. 2) in Appendix A. 


STEP (B) Compute a* = J P ] 

From Eq. (2.14), a f = B*a1 , where 

a* = <f>* H* ft (3.8) 

However we have that, 

Lemma 4: 

(7 - HtpK)H<f> = Hip (3.9) I 

Thus using Eq. (3.6) and the above lemma in Eq. (3.8), 

a f = <p m H*[I - HipK]* D~ 1 [I - HipK]T = ip* H*D~ l [I - HiPK]f 

Comparing this with Eq. (3.7) we see that q1 can be evaluated as an intermediate quantity in the 
0(Af) recursive algorithm for computing pi described in STEP (A). 


STEP (C) Compute A = 

Using Eq. (2.14) and Eq. (3.6), 

A = {[7 - 77V’7t']77<^S}*7? _1 {[7 - HipK]H<pB] 

= B*V’*77*D _1 77V>S = where Q = ip” H* D~ l Hip (3.10) 
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where Eq. (3.9) has been used to simplify the above expression. A recursive 0(M) procedure for the 
computation of is given in Eq. (A. 5) in Appendix A. Note that without the simplification resulting 
from the use of Eq. (3.9), the computation of A would be an 0{ M 3 ) process. 


STEP (D) Solve [ApAAp + A s A s A* s ]f = (Apa* p + A s dt J s ) - U for f 
Now, 

/ = [Ap\A* P + As\sA* s ]- l [{Apa s p + A s o^ s ) - U) (3.11) 

In this form this step is of 0(M%) complexity. However, when ( A p A p .4 * p ) is invertible, we can obtain 
an alternative expression for / by reexpressing Eq. (2.17) as follows: 

( M P 0 J' P A' P \( ? \ / T P \ 

0 Ms J* S A' S /? s = T S 

\ 0 A s Js -ApApA'p ) \ f ) \ u _ Ap& f p ) 

and consequently, 

( M P 0 J*pA* P \ ( p \ 

V 0 A S J S —ApApAp ) \ f ) 

( fp 

Ts + JsA* s (A P ApAp)~ 1 [U — Ap&tp] 

\ U - A P a f P 

From the above equation it follows that 

Ps = [. Ms^JsA*s{ApApA t p)- l A s Js)- 1 [Ts-r s A* s {ApAA*p)- l {Apa 1 p-U )] 

f = {ApAA'p)- l [{Apa } p + A s Js0s)-U] 

= (ApApA*p)- l [(A P a ! p + A s dc s ) - U], where a s = J S /3 S 

Note the similarity between the forms of Eq. (3.11) and the above equation for /. The computational 
cost of the above operation is a combination of the cost of inverting ApApA* p , and the 0(Af^) step of 
solving a square linear system of equations of size A fs- The cost of inverting ApApAp depends on its 
structure: its sparsity reflects the degree of coupling between the closed loops in the system. The cost 
is typically much less than the worst case of 0{N%). In many application domains such as robotics, 
ApApAp is in fact block diagonal and is thus invertible in 0(Me) steps [1]. In addition, for most 
applications J\f s <C Me, and this new formulation can lead to considerable computational savings. 

The inverse of [ApApAp + A 5 A 5 A 5 ] will not exist if [A P Jp A 5 J 5 ] is not of full rank, i.e., the 
configuration is such that the number of motion dofs for the system have changed. It is therefore 
necessary to reformulate the constraint equation Eq. (2.16) so as to preserve the full rank property. 
Such changes of rank can occur at kinematically singular configurations. 



STEP (E) Compute f3 6 — —M~ 1 J*A*f 
We have from Eq. (3.6) and Eq. (2.14) that 

P 6 = -[/ - Htl>K]*D- x [I - Hr!>K]H<t>BA'f 

Using Lemma 4 this simplifies to 

p 6 = -[/ - H1>K]*D- l H1>BA*f (3.12) 

The recursive 0(M) implementation of the above step is given in Eq. (A. 6 ) in Appendix A. 
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The overall complexity of this spatially recursive forward dynamics algorithm ranges between 0(M + 
Ms) + 0(M%) for the worst case and 0(M + Ms) + 0{Me) + 0(-A/|) in the best case. 

By treating the primary and secondary system as one system, which amounts to defining the quan- 
tities xp = diag(xp P , xps), H = diag(Hp,H s ) etc., and using the above results, the overall closed topology 
forward dynamics algorithm can be restated in the following form: 

^[I-HiltKYD-^I-biAAA'y'b^D-^I-HiPKft, where 6 = HipBA* (3.13) 

Note that when there are no closed loops in the overall system, A — 0, and the middle term reduces to J, 
and we recover the form for for the forward dynamics of tree topology systems in Eq. (3.7). 


4 Flexible Multibody Dynamics 


In this section we briefly describe the extensions to handle the case of flexible links. We use the serial chain 
discussed in Section 2.1 as an illustrative example, but now assume that the links in the chain are flexible. 
It is assumed (without losing any generality) that finite element models are available for all the links, and 
in particular, the k th link is characterized by: n k node points with the location of of the j th node denoted 
Q k (j)’s, the vector of displacement variables £ 7?. 6n ‘ , a free-free mass-matrix m k £ 7?. r ”‘ k x6n * , a stiffness 
matrix K k € ft 6n * x6n *. The ordering of the nodes is such that Q k ( 1) is on the same element as 0*_i and 
Qk(n k ) is on the same element as O k . Treating the k th link as being pinned at O k , this implies that 
u k (n k ) = 0, and thus the true flexible dofs are given by the vector u k = h k u{, where [l , 0] . Note that 
u k € ft 6 *"*" 1 ) and h k £ 


With V (k) denoting the spatial velocity of the k th link at O k , 

V(k) = <f>*(k + 1, Jb)V(fc + 1) + H*(k)f3(k) + 4>* (Q*+i ( 1) , ( 1) 

= f(k + 1, k)V(k + 1) + H-(k)/}(k) + C* k+1 hlu k+1 (4.1) 

where C* k+l = fe*(<2t + i(l),0*), 0, ••• 0] G ft 6x6 "‘ (4.2) 

Thus, 

V = <f[H'p + C*h*u] 

with C defined as the block matrix with C 2 to C„ along its first block subdiagonal, and h is the block 
diagonal matrix with j th block diagonal element being hj. Let V k € 7l 6nk denote the vector of spatial 
velocities on the k ih link at the n k node points. Then 

V/ = B k V(k) 4- h k u k , where B k = [<)>{O k ,Q k {\)),- ■ ■ A{O k ,Q k {n k ))} £ 77. 6x6n * 

=» V ! = AT + + C’A’u] + k'tt = + [/ + 

= [/ + B'H*]X, where X= (“) (4.3) 

B denotes the block diagonal matrix with the Bk s along its diagonal. We have used the facts that, 

BkCk — <f>{k y A: — 1) => BC = £$ => BC<p = <f> — I => B[I + C<pB] — <pB 

Note that X is the vector of motion dofs for the serial links and includes both the rigid and flexible dof 

parameters. 


Using Eq. (4.3), the kinetic energy for the whole chain is given by 
T=^V')*mV' = \X'M J X, where M 1 = ( ) [/ + C<j>B]m[I + C<f>B]* ( ) 
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M* is the mass matrix for the flexible serial chain. Given this factored form for the mass matrix, similar 
techniques to those used for the rigid multibody case in the earlier sections result in alternate factorizations 
and inversion of the mass matrix, and recursive forward dynamics algorithms. The reader is referred to [8] 
for additional details. Just as for the rigid multibody case, the algorithms for flexible serial chains directly 
extend to the flexible general topology multibody systems. 


5 Conclusions 


This paper describes the Spatial Operator Algebra Framework for the dynamics of general multibody systems. 
Based on their rate of time-variation, the multibody system is partitioned into a primary subsystem, a 
secondary subsystem and the set of closure constraints. This allows the development of forward dynamics 
algorithms which are not only recursive and efficient, but also capable of easily coping with time varying 
multibody systems. The solution procedure consists of a sequence of steps on parallel paths involving the 
dynamics of the spanning trees for the primary and secondary systems. The two paths come together for 
one step in order to compute the constraint forces. Using the spatial algebra techniques to develop novel 
factorizations of the mass matrix and operator identities, efficient recursive algorithms for carrying out each 
of these steps is developed. The overall algorithm does not require the computation of the mass matrix, and 
its complexity is linear in the number of dofs for the tree systems. In addition, the impact on the complexity 
of the algorithm, of the degree of coupling among the closed loops in the system topology is made clear, 
and it is shown that the in the best circumstance, the algorithmic complexity is also linear in the number 
of closure constraint equations. During the development, an 0{M) forward dynamics algorithm for tree 
topology systems is also developed. For the sake of clarity, the focus of much of the paper was on multibody 
systems with rigid links. However the extensions necessary to deal with flexible elements are discussed. 
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A Appendix 


Based on the special structure of etc., it is possible to evaluate many of the dynamical expressions 
in a recursive manner and we describe some recursive algorithms in this appendix. First we define some 
notational shorthand to simplify the description of the algorithms that follow: 


+ 1) 

y(»j + l. n i) 

yUj.OjMOj) 


• 0j)aj(0j)y*(lj , 0,-) 


X] t/(lj,n m )x(n m ) 

m € * " 1 ( J ) 


^2 y(h’ n ™) x ( n ™)y*(h> n m) 


where $/(.,.) and :r(.) stand for some appropriate arrays. Thus wherever a term with indices as in the left 
column appears, its meaning is actually given by the corresponding term in the column on the right. !pr 


• A recursive method for the computation of the block diagonal elements of P as defined by Eq. (3.1) 
and the entries of D ) G, K , and r defined in Eq. (3.2) are given by: 

( for j = I t 


If l- 1 

0) = 

= 0, then P(0j) = 0 

for k = 1: • • • 

n j 


m 


rf>(k,k-l)P(k-W(k,k-l) + M(k) 

m 

= 

H(k)P(k)H*(k) 

G(k) 

= 

P(k)H* (k)D~ 1 (k) 

T{k) 


I - G(k)H(k) 

W + I,k) 

= 

ij)(k + 1 ,k)7(k) 

K(k+l,k) 

= 

<j>(k + \,k)G(k) 

end loop 




end loop 

• The recursive computation of fit = [/ — Hij>K]*D~*[I — Hij>K]T in Eq. (3.7) in STEP (A) can be 
carried out via the 0(Af) tree topology forward dynamics algorithm described below. It also results in 
the computation of a f — D~ l [I - HipK]T required in STEP (B) as an intermediate quantity. 

r 

for j = 1 ••■l 

' If » _1 (j) = 0) then z(0j) = 0,T(0j) = 0 

for k = lj • • • nj 

< I z(k) = t/’(k,k-l)z(k-l) + K(k,k-l)f(k-l) 

| c(Jfe) = T(k) — H(k)z(k) 

u(k) - D-'ikyik) 

V end loop 
i end loop 
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< 


(A.2) 


& (nt + 1 ) = 0 

for j = t - 1 

for k = nj • • • 1 j 

& f (k) = $*(k+l t k)&t(k + l) + H*(k)v(k) 

0f (k) = v(k)-K*(k + l)af(k + l) 
end loop 
l end loop 

• STEP (C) requires the computation of A = B*QB. In order to obtain a O(Af) recursive scheme for 
the computation of ft we first define the matrix T as the one satisfying the equation: 

H m D~ l H = r -£$?£+ (A. 3) 

T as defined above is a block diagonal matrix and its elements can be computed recursively. We now 
obtain the following decomposition of ft. 

Lemma 5: 

ft = T + ^*T + T^ (A. 4) ■ 


Noting that is strictly lower triangular, we can then recognize that T as nothing but the diagonal 
elements of ft. We now present a recursive scheme to compute the block diagonal elements of T and of 


ft. 


T(n/ + 1) = 0 
for j = 11 


for k = nj • - - 1 j 

T(Jb) = + 1, *r)T(* + l)^(Ar + 1, Jb) + (Ar)Z^“ 1 (Jb)i7(*r) 

' i [ n(M) = T(ft) 

I for m = k — 1 • • • lj 

j ft(fc,m) = ft*(m, k) = T(fc, m + l)^(m + 1, m) 

{ end loop 
end loop 
, end loop 


The above recursion yields the elements ftj on the block diagonal of ft. Since ft is symmetric, the 
off-diagonal elements satisfy Qjj = ft*j, and can be computed from the diagonal elements as follows, 
ftjj for / G 1 • • • (j — 1) can be obtained via the following recursive scheme: 


if i p (l) = j for some p > 0 
fork = nj lj 

for m = nj * • • lj 

ft(fc, m) = ft*(m 1 Jfc) = ft(ib ) l i )^(l i) m) (A.5) 

end loop 
end loop 

else 

ft ;> , = ft^ = 0 

k end if 
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J 


• The 0(Af) recursive implementation of (3 6 = — [I — HtpK] m D“ l HrpBA* f in Eq. (3.12) in Step (e) is 
given below: 


for j 


i 


Define x = — BA* f 
= l -l 

If t-'U) = 0, then z(0j) = 0,1(0,) = 0 
for k = 1 j • • • nj 


*(*) = 4>(k,k-l)z(k-l) + K(k,k-l)£(k-l) 

e(t) = -H{k)z(k) 
v(k) = D-'(#) 

end loop 
end loop 


a(n< + 1) = 0 

for j = l - 1 

r 

for k = nj • • • lj 

< a(i fc) = i/>*{k + 1, k)a(k + 1) + H*(k)u(k) 

j3 f (k) = v(k)-K*(k + l)a(k+l) 

end loop 
k end loop 


(A.6) 



Figure 1: Illustration of Iink/branch numbering convention for multibody system 
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